#pragma config(Hubs,  S1, HTMotor,  HTMotor,  none,     none)
#pragma config(Hubs,  S2, HTServo,  HTMotor,  HTMotor,  none)
#pragma config(Sensor, S1,     ,               sensorI2CMuxController)
#pragma config(Sensor, S2,     ,               sensorI2CMuxController)
#pragma config(Sensor, S3,     tLimits,        sensorI2CCustom9V)
#pragma config(Sensor, S4,     tIRBeacon,      sensorHiTechnicIRSeeker1200)
#pragma config(Motor,  motorA,          mTow,          tmotorNXT, PIDControl, encoder)
#pragma config(Motor,  motorB,          mLeftTow,      tmotorNXT, PIDControl, encoder)
#pragma config(Motor,  motorC,          mRightTow,     tmotorNXT, PIDControl, reversed, encoder)
#pragma config(Motor,  mtr_S1_C1_1,     dLeft1,        tmotorTetrix, openLoop, encoder)
#pragma config(Motor,  mtr_S1_C1_2,     dLeft2,        tmotorTetrix, openLoop)
#pragma config(Motor,  mtr_S1_C2_1,     dRight1,       tmotorTetrix, openLoop, reversed, encoder)
#pragma config(Motor,  mtr_S1_C2_2,     dRight2,       tmotorTetrix, openLoop, reversed)
#pragma config(Motor,  mtr_S2_C2_1,     aBallTip,      tmotorTetrix, openLoop)
#pragma config(Motor,  mtr_S2_C2_2,     aUExtend,      tmotorTetrix, openLoop, reversed)
#pragma config(Motor,  mtr_S2_C3_1,     aBallPickup,   tmotorTetrix, openLoop, reversed)
#pragma config(Motor,  mtr_S2_C3_2,     aLExtend,      tmotorTetrix, openLoop, reversed)
#pragma config(Servo,  srvo_S2_C1_1,    sBallDrop,            tServoStandard)
#pragma config(Servo,  srvo_S2_C1_2,    sTowGoal,             tServoStandard)
#pragma config(Servo,  srvo_S2_C1_3,    sIRSeeker,            tServoStandard)
#pragma config(Servo,  srvo_S2_C1_4,    servo4,               tServoNone)
#pragma config(Servo,  srvo_S2_C1_5,    servo5,               tServoNone)
#pragma config(Servo,  srvo_S2_C1_6,    servo6,               tServoNone)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

#include "JoystickDriver.c"  //Include file to "handle" the Bluetooth messages.
#include "FTC_Toolkit.h"

bool testVal = false;
int TestSen = 0;

// Initialize Code
void initializeRobot()
{
}

// Main Task

task main()
{
  initializeRobot();

  //waitForStart();   // wait for start of tele-op phase

  while (true)
  {
  	TestSen = HTSPBreadIO(tLimits, 0b01101111);
  }
}
